"""
Description: 实时显示进行手眼标定的 Vive Tracker 数据 (将其转换为机器人坐标系下的姿态) 。

Author: Jin Liu
Date: 2025/06/18
"""

from pathlib import Path

from ezrobot.robot import UR5
from ezrobot.sensor import ViveTracker, SensorManager
from ezrobot.sim import RobotClient
from ezrobot.utils import HandEyeConfig
from ezgl import tb

BASE_DIR = Path(__file__).parent

if __name__ == '__main__':
    robot = UR5(arm_ip="192.168.77.36", with_gripper=False)
    robot.teach_mode()
    tracker = SensorManager(ViveTracker(device_index=1), frequency=30, buffer_size=10, streaming=True)
    client = RobotClient()
    client.clear_items()
    handeye_config = HandEyeConfig.load(BASE_DIR / "../configs/handeye_config.yaml")

    def on_timeout():
        client.add_pose_axis(handeye_config.base2map * tracker.get_data() * handeye_config.hand2eye.inverse())

        # visualize
        joints = robot.get_joints_rad()
        client.set_joint(joints)

    with tb.window("control", None, 10, size=(500, 500), pos=(400, 0), frameless=False):
        tb.add_text("text", text="visualize vive tracker")
        tb.add_stretch(1)

        tb.add_timer("update", interval_ms=200, callback=on_timeout)

    tb.exec()


